import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
import serial
import struct
import time
from crccheck.crc import CrcXmodem  # 使用 crccheck 库
import sys

# 串口配置函数
def setup_serial_connection():
    try:
        return serial.Serial('/dev/ttyS6', 115200, timeout=1)
    except serial.SerialException:
        return None

def calc_crc(data: bytes) -> int:
    crc = 0xFFFF  # 初始值
    for byte in data:
        crc ^= byte  # 将数据字节与CRC异或
        for _ in range(8):  # 对每一位进行处理
            if (crc & 0x0001):  # 如果最低位为1
                crc = (crc >> 1) ^ 0xA001  # 右移一位，与多项式A001异或
            else:
                crc >>= 1  # 否则仅右移一位
    return crc & 0xFFFF

def build_message(data_id, data):
    """根据协议构造报文"""
    
    # 报文头固定为 0xA5
    head = 0xA5
    
    # 数据ID：用户传入
    data_id = data_id
    
    # 数据域长度
    data_len = len(data)
    
    # 数据域内容
    data_content = bytes(data)
    
    # CRC 校验：数据部分 + 数据ID + 数据长度 + 数据域内容
    crc_data = bytes([head, data_id, data_len]) + data_content
    print(f"crc: {crc_data.hex()}")
    # crc = calculate_crc16(crc_data)
    # print(crc)
    crc = calc_crc(crc_data)
    print(crc)
    
    # 将 CRC 分割成两个字节（低字节在前）
    crc_low = crc & 0xFF
    crc_high = (crc >> 8) & 0xFF
    
    # 构造完整的报文
    message = struct.pack('B', head) + struct.pack('B', data_id) + struct.pack('B', data_len) + data_content + struct.pack('B', crc_low) + struct.pack('B', crc_high)
    print(message.hex())
    return message

def send_message(serial_port, message):
    """通过串口发送报文"""
    try:
        bytes_written = serial_port.write(message)
        print(f"message len: {len(message)}\n成功写入 {bytes_written} 个字节")
    except serial.SerialException as e:
        print(f"无串口: {e}")

class SerialCommunicationNode(Node):
    def __init__(self):
        super().__init__('send_serial_node')
        self.get_logger().info("serial_comm_node has been started.")
        self.droneid=int(sys.argv[1])
        # 串口连接
        self.ser = setup_serial_connection()
        self.mode = 1
        if(self.ser == None):
            self.mode = 0
        #print(self.ser)
        # print(f"/drone{self.droneid}/velocity_and_position")
        
        print(f"/drone{self.droneid}/setpoint_raw/local")
        # 订阅位置和速度话题
        self.subscription = self.create_subscription(
            Float64MultiArray,
            f"/drone{self.droneid}/setpoint_raw/local",
            self.position_velocity_callback,
            10  # QoS设置，类似于ROS1中的10Hz频率
        )

        self.longitude = int(0)  # 假设单位为度*1E7
        self.latitude = int(0)
        self.altitude = int(0)  # 假设单位为厘米
        self.velocity_east = int(0)  # 假设单位为cm/s
        self.velocity_north = int(0)
        self.velocity_up = int(0)
       

    def send2serial(self):
        # 按小端序构造数据内容
        # 对每个多字节字段的字节顺序进行小端排列（反转原来大端的字节顺序）
        print(f"Get from Algorithm: {self.longitude}--{self.latitude}--{self.altitude}")
        data = (
            list(struct.pack('<i', self.longitude)) +  # 小端序的经度
            list(struct.pack('<i', self.latitude)) +  # 小端序的纬度
            list(struct.pack('<i', self.altitude)) +  # 小端序的海拔
            list(struct.pack('<h', self.velocity_east)) +  # 小端序的东向速度
            list(struct.pack('<h', self.velocity_north)) +  # 小端序的北向速度
            list(struct.pack('<h', self.velocity_up))  # 小端序的天向速度
        )
        data_id = 105
        
        
        # 构造报文
        message = build_message(data_id, data)
        
        # 发送报文
        if(self.mode):
            send_message(self.ser, message)
        
        # 打印发送的报文（十六进制）
        # self.get_logger().info(f"Sent message: {message.hex()}")
    
    def position_velocity_callback(self, msg):
        # 从消息中获取位置和速度数据
        # 假设 Float32MultiArray 中的前3个是期望位置 (x, y, z)，后3个是速度 (vx, vy, vz)
        # msg.data 格式: [longitude, latitude, altitude, velocity_east, velocity_north, velocity_up]
        
        if len(msg.data) < 6:
            self.get_logger().warn("Received data is incomplete!")
            return
        
        # 提取数据
        self.longitude = int(msg.data[0] * 1e7)  # 假设单位为度*1E7
        self.latitude = int(msg.data[1] * 1e7)
        self.altitude = int(msg.data[2] * 100)  # 假设单位为厘米
        self.velocity_east = int(msg.data[3] * 100)  # 假设单位为cm/s
        self.velocity_north = int(msg.data[4] * 100)
        self.velocity_up = int(msg.data[5] * 100)
        print(f"point GNSS: {self.longitude/1e7}--{self.latitude/1e7}--{self.altitude/1e2}")
        print(f"velocity: {self.velocity_east/1e2}--{self.velocity_north/1e2}--{self.velocity_up/1e2}")
        self.send2serial()
        
        
    
    def __del__(self):
        # 清理工作：关闭串口
        if(self.mode):
            self.ser.close()

    

def main(args=None):
    rclpy.init(args=args)

    # 创建节点
    node = SerialCommunicationNode()
    
    try:
        # Spin the node to keep it alive and process callbacks
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    
    # 关闭节点
    
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
